#include "servoagent.h"
#include "log/log.h"
#include "soem/soemethercat.h"
#include "../app/system/systemconfig.h"
#include <QMap>
#include <QDebug>

ServoAgent::ServoAgent()
{    
};

ServoAgent::~ServoAgent()
{    
    disInitSoemEthercat();
};

bool ServoAgent::getErrstr(int errCode , QString&  errStr)
{
    switch (errCode) {
    case -1 : {
        errStr =tr("The number of servo devices is incorrect");
        return true;
    }break;
    case -2 : {
        errStr =tr("Servo device PDO remapping failed");
        return true;
    }break;
    case -3 : {
        errStr =tr("Servo device PDO output acquisition failed");
        return true;
        }break;
    case -4 : {
        errStr =tr("Not all slaves reached operational state");
        return true;
    }break;
    case -5 : {
        errStr =tr("ec_config_init failed");
        return true;
    }break;
    case -6 : {
        errStr =tr("ec_init failed");
        return true;
    }break;
    default:{
        return false;
    }break;
    }
}

bool ServoAgent::initSoemEthercat(const std::string & ifname)
{
    std::string ifbuf = "\\Device\\NPF_";
    ifbuf.append(ifname);
    int errCode = initEC(ifbuf.c_str());

    if(errCode>0){
        isInited = true;
        return true;
    }else{
        QString errStr;
        if(getErrstr(errCode , errStr)){
            HIM_ERROR(errStr.toStdString());
        }
        return false;
    }
}

bool ServoAgent::disInitSoemEthercat()
{
    try {
        stopEC();
        isInited = false;
        return true;
    } catch (std::exception& e) {
        throw e.what();
        return false;
    }
}

void ServoAgent::loadAllServoConfig()
{
    servoParasMap[MANUFACTURE].servoNO = SYSTEMCONFIG.getLongValue(PARAMETER,"lServoNO");
    servoParasMap[MANUFACTURE].servoStepsPerMM = SYSTEMCONFIG.getLongValue(PARAMETER,"lservoStepsPerMM");

    servoParasMap[MANUFACTURE].servoSpeed = SYSTEMCONFIG.getDoubleValue(PARAMETER,"lServoSpeed")*servoParasMap[MANUFACTURE].servoStepsPerMM;
    servoParasMap[MANUFACTURE].servoAcceleration = SYSTEMCONFIG.getDoubleValue(PARAMETER,"lServoAcceleration")*servoParasMap[MANUFACTURE].servoStepsPerMM;
    servoParasMap[MANUFACTURE].servoPowerRev = SYSTEMCONFIG.getLongValue(PARAMETER,"lServoPowerRev");
    servoParasMap[MANUFACTURE].servoPower = SYSTEMCONFIG.getLongValue(PARAMETER,"lServoPower");
    servoParasMap[MANUFACTURE].servoSpeedMax = SYSTEMCONFIG.getDoubleValue(PARAMETER,"lServoSpeedMax")*servoParasMap[MANUFACTURE].servoStepsPerMM;

    servoParasMap[FEED].servoNO = SYSTEMCONFIG.getLongValue(PARAMETER,"rServoNO");
    servoParasMap[FEED].servoStepsPerMM = SYSTEMCONFIG.getLongValue(PARAMETER,"rservoStepsPerMM");

    servoParasMap[FEED].servoSpeed = SYSTEMCONFIG.getDoubleValue(PARAMETER,"rServoSpeed")*servoParasMap[FEED].servoStepsPerMM;  
    servoParasMap[FEED].servoAcceleration = SYSTEMCONFIG.getDoubleValue(PARAMETER,"rServoAcceleration")*servoParasMap[FEED].servoStepsPerMM;
    servoParasMap[FEED].servoPowerRev = SYSTEMCONFIG.getLongValue(PARAMETER,"rServoPowerRev");
    servoParasMap[FEED].servoPower = SYSTEMCONFIG.getLongValue(PARAMETER,"rServoPower");
    servoParasMap[FEED].servoSpeedMax = SYSTEMCONFIG.getDoubleValue(PARAMETER,"RServoSpeedMax")*servoParasMap[FEED].servoStepsPerMM;  

    servoParasMap[POWDER].servoNO = SYSTEMCONFIG.getLongValue(PARAMETER,"pcServoNO");
    servoParasMap[POWDER].servoStepsPerMM = SYSTEMCONFIG.getLongValue(PARAMETER,"pcStepsPerMM");

    servoParasMap[POWDER].servoSpeed = SYSTEMCONFIG.getDoubleValue(PARAMETER,"pcAxisMoveSpeed")*servoParasMap[POWDER].servoStepsPerMM;
    servoParasMap[POWDER].servoSpeedRev = SYSTEMCONFIG.getDoubleValue(PARAMETER,"pcAxisCoverSpeed")*servoParasMap[POWDER].servoStepsPerMM;
    servoParasMap[POWDER].servoAcceleration = SYSTEMCONFIG.getDoubleValue(PARAMETER,"pcAxisAcceleration")*servoParasMap[POWDER].servoStepsPerMM;
    servoParasMap[POWDER].servoPowerRev = SYSTEMCONFIG.getLongValue(PARAMETER,"PcAxisPowerRev");
    servoParasMap[POWDER].servoPower = SYSTEMCONFIG.getLongValue(PARAMETER,"pcAxisPower");
    servoParasMap[POWDER].servoSpeedMax = SYSTEMCONFIG.getDoubleValue(PARAMETER,"PcServoSpeedMax")*servoParasMap[POWDER].servoStepsPerMM;

    pcMulValue = SYSTEMCONFIG.getDoubleValue(PARAMETER,"pcMulValue");    

    feedPosMax = SYSTEMCONFIG.getLongValue(PARAMETER,"sizeZSup");
    manPosMax = SYSTEMCONFIG.getLongValue(PARAMETER,"sizeZMan");
    powerPosMax = SYSTEMCONFIG.getLongValue(PARAMETER,"pcAxisMin");
}

bool ServoAgent::servoEnable(SERVONUM num)
{
    if(drive_setParas(servoParasMap[num].servoNO,servoParasMap[num].servoSpeedMax,
                      servoParasMap[num].servoPower,servoParasMap[MANUFACTURE].servoPowerRev)
            &&drive_enable(servoParasMap[num].servoNO)){

        switch (num) {
        case FEED : {
            HIM_INFO(tr("Powder supply servo enable").toStdString());
        }break;
        case MANUFACTURE : {
            HIM_INFO(tr("Forming servo enable").toStdString());
        }break;
        case POWDER : {
            HIM_INFO(tr("Scraper servo enable").toStdString());
        }break;
        default:break;
        }
        return true;

    }else{

        switch (num) {
        case FEED : {
            HIM_INFO(tr("Powder supply servo enable failed").toStdString());
        }break;
        case MANUFACTURE : {
            HIM_INFO(tr("Forming servo enable failed").toStdString());
        }break;
        case POWDER : {
            HIM_INFO(tr("Scraper servo enable failed").toStdString());
        }break;
        default:break;
        }
        return false;
    }
}

bool ServoAgent::servoDisEnable(SERVONUM num)
{
    if(drive_disenable(servoParasMap[num].servoNO)){
        HIM_INFO("disenable servo");
        return true;
    }else{
        return false;
    }
}

bool ServoAgent::servoReset(SERVONUM num)
{
    if(drive_reset(servoParasMap[num].servoNO)){
        return true;
    }else{
        return false;
    }
}

bool ServoAgent::moveStop(SERVONUM num)
{
    if(servoMoveStop(servoParasMap[num].servoNO)){
        return true;
    }else{
        return false;
    }
}

bool ServoAgent::moveJog(SERVONUM num ,MOTIONTYPE motionType , double speed)
{   
    int dir = 0;
    switch (motionType) {
    case UP : {
        dir = -jogUnitDis;
    } break;
    case DOWN : {
        dir = jogUnitDis;
    } break;
    default:break;
    }

    if(servoMoveJog(servoParasMap[num].servoNO,dir*servoParasMap[num].servoStepsPerMM/1000,
                    speed*servoParasMap[num].servoStepsPerMM,
                    servoParasMap[num].servoAcceleration,servoParasMap[num].servoAcceleration)){
        return true;
    }else{
        return false;
    }
}

bool ServoAgent::moveJog(SERVONUM num , double relativeDistance)
{
    if(servoMoveJog(servoParasMap[num].servoNO,relativeDistance*servoParasMap[num].servoStepsPerMM,
                    servoParasMap[num].servoSpeed,servoParasMap[num].servoAcceleration,servoParasMap[num].servoAcceleration)){
        return true;
    }else{
        return false;
    }
}

bool ServoAgent::moveAbs(SERVONUM num , double pos , double speed)
{
    if(servoMoveAbs(servoParasMap[num].servoNO,pos*servoParasMap[num].servoStepsPerMM,
                    speed*servoParasMap[num].servoStepsPerMM,
                    servoParasMap[num].servoAcceleration,servoParasMap[num].servoAcceleration)){
        return true;
    }else{
        return false;
    }
}

bool ServoAgent::moveZero(SERVONUM num, double speed)
{
    if(servoMoveAbs(servoParasMap[num].servoNO,0,speed*servoParasMap[num].servoStepsPerMM,
                    servoParasMap[num].servoAcceleration,servoParasMap[num].servoAcceleration)){
        return true;
    }else{
        return false;
    }
}

bool ServoAgent::setZero(SERVONUM num)
{
    if(servoSetZero(servoParasMap[num].servoNO)){
        return true;
    }else{
        return false;
    }
}

bool ServoAgent::scraperMoveLeft(double speed)
{
    if(servoMoveAbs(servoParasMap[POWDER].servoNO,powerPosMax*servoParasMap[POWDER].servoStepsPerMM,speed*servoParasMap[POWDER].servoStepsPerMM,
                    servoParasMap[POWDER].servoAcceleration,servoParasMap[POWDER].servoAcceleration)){
        return true;
    }else{
        return false;
    }
}

bool ServoAgent::scraperMoveRight(double speed)
{
    if(servoMoveAbs(servoParasMap[POWDER].servoNO,0,speed*servoParasMap[POWDER].servoStepsPerMM,
                    servoParasMap[POWDER].servoAcceleration,servoParasMap[POWDER].servoAcceleration)){
        return true;
    }else{
        return false;
    }
}

bool ServoAgent::scraperSpreading(double ratio)
{
    if(servoMoveAbs(servoParasMap[POWDER].servoNO,powerPosMax*servoParasMap[POWDER].servoStepsPerMM,servoParasMap[POWDER].servoSpeedRev*ratio,
                    servoParasMap[POWDER].servoAcceleration,servoParasMap[POWDER].servoAcceleration)){
        return true;
    }else{
        return false;
    }
}

bool ServoAgent::scraperToZore(double ratio)
{
    if(servoMoveAbs(servoParasMap[POWDER].servoNO,0,servoParasMap[POWDER].servoSpeed*ratio,
                    servoParasMap[POWDER].servoAcceleration,servoParasMap[POWDER].servoAcceleration)){
        return true;
    }else{
        return false;
    }
}
